/*
 * MatrixTools.cpp
 *
 *  Created on: 2013-1-6
 *  Author: scott.cgi
 */
#include "MatrixTools.h"
#include <string.h>
#include <math.h>
#define PI 3.1415926535897932384626433832795f
void px_loadIdentityM(Matrix result) {
	int i;
    memset(result, 0x0, sizeof(Matrix));
    for(i = 0; i < 16; i += 5) {
    	result[i] = 1.0f;
    }
}

void px_multiplyMM(Matrix r, Matrix leftM, Matrix rightM) {
	int i , j;
	for (i = 0; i < 4; i++) {
		register const float rmi0 = rightM[/*0 + 4 * i*/ 4 * i];

		register float ri0 = leftM[/*0 + 4 * 0*/0] * rmi0;
		register float ri1 = leftM[/*1 + 4 * 0*/1] * rmi0;
		register float ri2 = leftM[/*2 + 4 * 0*/2] * rmi0;
		register float ri3 = leftM[/*3 + 4 * 0*/3] * rmi0;

		for (j = 1; j < 4; j++) {
			register const float rij = rightM[j + 4 * i];

			ri0 += leftM[/*0 + 4 * j*/4 * j] * rij;
			ri1 += leftM[1 + 4 * j] * rij;
			ri2 += leftM[2 + 4 * j] * rij;
			ri3 += leftM[3 + 4 * j] * rij;
		}

		r[/*0 + 4 * i*/4 * i] = ri0;
		r[1 + 4 * i] = ri1;
		r[2 + 4 * i] = ri2;
		r[3 + 4 * i] = ri3;
	}
}

void px_orthoM(Matrix result, float left, float right, float bottom, float top, float near, float far) {

    if (right == left || top == bottom || far == near) {
    	
    	return;
    }
	do 
	{  
	float width  = 1.0f / (right - left);
	float height = 1.0f / (top - bottom);
	float depth  = 1.0f / (far - near);

	float x =  2.0f * (width);
	float y =  2.0f * (height);
	float z = -2.0f * (depth);

	float tx = -(right + left) * width;
	float ty = -(top + bottom) * height;
	float tz = -(far + near)   * depth;

	result[0] = x;
	result[5] = y;
	result[10] = z;

	result[12] = tx;
	result[13] = ty;
	result[14] = tz;

	result[15] = 1.0f;

	result[1] = 0.0f;
	result[2] = 0.0f;
	result[3] = 0.0f;
	result[4] = 0.0f;
	result[6] = 0.0f;
	result[7] = 0.0f;
	result[8] = 0.0f;
	result[9] = 0.0f;
	result[11] = 0.0f;
	} while (0);

}

void px_frustumM(Matrix result, float left, float right, float bottom, float top, float near, float far) {

    if(right == left || top == bottom || near == far || near <= 0.0f || far <= 0.0f) {
    	
    	return;
    }
	do 
	{ 
	float width  = 1.0f / (right - left);
	float height = 1.0f / (top - bottom);
	float depth  = 1.0f / (near - far);

	float x = 2.0f * (near * width);
	float y = 2.0f * (near * height);

	float A = 2.0f * ((right + left) * width);
	float B = (top + bottom) * height;
	float C = (far + near) * depth;
	float D = 2.0f * (far * near * depth);

	result[0] = x;
	result[5] = y;
	result[8] = A;

	result[ 9] = B;
	result[10] = C;
	result[14] = D;

	result[11] = -1.0f;
	result[ 1] = 0.0f;
	result[ 2] = 0.0f;
	result[ 3] = 0.0f;
	result[ 4] = 0.0f;
	result[ 6] = 0.0f;
	result[ 7] = 0.0f;
	result[12] = 0.0f;
	result[13] = 0.0f;
	result[15] = 0.0f;
	} while (0);

 
}

void px_rotateM(Matrix result, float angle, float x, float y, float z) {

	float s,c;
    result[3] = 0;
    result[7] = 0;
    result[11]= 0;
    result[12]= 0;
    result[13]= 0;
    result[14]= 0;
    result[15]= 1;

    angle *= sqrtf(PI / 180.0f);

    s = sinf(angle);
    c = cosf(angle);

    if (1.0f == x && 0.0f == y && 0.0f == z) {
        result[5] = c;   result[10]= c;
        result[6] = s;   result[9] = -s;
        result[1] = 0;   result[2] = 0;
        result[4] = 0;   result[8] = 0;
        result[0] = 1;
    } else if (0.0f == x && 1.0f == y && 0.0f == z) {
        result[0] = c;   result[10]= c;
        result[8] = s;   result[2] = -s;
        result[1] = 0;   result[4] = 0;
        result[6] = 0;   result[9] = 0;
        result[5] = 1;
    } else if (0.0f == x && 0.0f == y && 1.0f == z) {
        result[0] = c;   result[5] = c;
        result[1] = s;   result[4] = -s;
        result[2] = 0;   result[6] = 0;
        result[8] = 0;   result[9] = 0;
        result[10]= 1;
    } else {
        float len = sqrtf(x * x + y * y + z * z);

        if (1.0f != len) {
            float recipLen = 1.0f / len;

            x *= recipLen;
            y *= recipLen;
            z *= recipLen;
        }
		{
        float nc = 1.0f - c;
        float xy = x * y;
        float yz = y * z;
        float zx = z * x;
        float xs = x * s;
        float ys = y * s;
        float zs = z * s;

		result[0]  = x * x * nc + c;
		result[4]  = xy * nc - zs;
		result[8]  = zx * nc + ys;
		result[1]  = xy * nc + zs;
		result[5]  = y * y * nc + c;
		result[9]  = yz * nc - xs;
		result[2]  = zx * nc - ys;
		result[6]  = yz * nc + xs;
		result[10] = z * z * nc + c;
		}
    }
}

void px_translateM(Matrix result, float tx, float ty, float tz) {
	int i;
	for (i = 0; i < 4; i++) {
		result[12 + i] += result[i] * tx + result[4 + i] * ty + result[8 + i] * tz;
	}
}
void px_scaleM(Matrix result, float sx, float sy, float sz) {
	int i;
	for (i = 0; i < 4; i++) {
		result[i] *= sx;
		result[4 + i] *= sy;
		result[8 + i] *= sz;
	}
}


void px_multiplyMV(Vector result, Matrix matrix, float x, float y, float z, float w) {
//    result[0] = matrix[0 + 4 * 0] * x + matrix[0 + 4 * 1] * y + matrix[0 + 4 * 2] * z + matrix[0 + 4 * 3] * w;
//    result[1] = matrix[1 + 4 * 0] * x + matrix[1 + 4 * 1] * y + matrix[1 + 4 * 2] * z + matrix[1 + 4 * 3] * w;
//    result[2] = matrix[2 + 4 * 0] * x + matrix[2 + 4 * 1] * y + matrix[2 + 4 * 2] * z + matrix[2 + 4 * 3] * w;
//    result[3] = matrix[3 + 4 * 0] * x + matrix[3 + 4 * 1] * y + matrix[3 + 4 * 2] * z + matrix[3 + 4 * 3] * w;
	int i;
    for(i = 0; i < 4; i++) {
    	result[i] = matrix[i/* + 4 * 0*/] * x + matrix[i + 4/*4 * 1*/] * y + matrix[i + 8/*4 * 2*/] * z + matrix[i + 12/*4 * 3*/] * w;
    }
}


void px_multiplyMV3(Vector result, Matrix matrix, float x, float y, float z) {
	int i;
    for(i = 0; i < 3; i++) {
    	result[i] = matrix[i/* + 4 * 0*/] * x + matrix[i + 4/*4 * 1*/] * y + matrix[i + 8/*4 * 2*/] * z + matrix[i + 12/*4 * 3*/];
    }
}

void px_lookAtM(Matrix result,
					float eyeX, float eyeY, float eyeZ,
					float centerX, float centerY, float centerZ,
					float upX, float upY, float upZ) {

	// See the OpenGL GLUT documentation for gluLookAt for a description
	// of the algorithm. We implement it in a straightforward way:

	float fx = centerX - eyeX;
	float fy = centerY - eyeY;
	float fz = centerZ - eyeZ;
	float sx,sy,sz,rls,ux,uy,uz;
	// Normalize f
	float rlf = 1.0f / sqrtf(fx * fx + fy * fy + fz * fz);
	fx *= rlf;
	fy *= rlf;
	fz *= rlf;

	// compute s = f x up (x means "cross product")
	sx = fy * upZ - fz * upY;
	sy = fz * upX - fx * upZ;
	sz = fx * upY - fy * upX;

	// and normalize s
    rls = 1.0f / sqrtf(sx * sx + sy *sy + sz * sz);
	sx *= rls;
	sy *= rls;
	sz *= rls;

	// compute u = s x f
	ux = sy * fz - sz * fy;
	uy = sz * fx - sx * fz;
	uz = sx * fy - sy * fx;

	result[0] = sx;
	result[1] = ux;
	result[2] = -fx;
	result[3] = 0.0f;

	result[4] = sy;
	result[5] = uy;
	result[6] = -fy;
	result[7] = 0.0f;

	result[8] = sz;
	result[9] = uz;
	result[10] = -fz;
	result[11] = 0.0f;

	result[12] = 0.0f;
	result[13] = 0.0f;
	result[14] = 0.0f;
	result[15] = 1.0f;

	px_translateM(result, -eyeX, -eyeY, -eyeZ);
}


